#include "rclcpp/rclcpp.hpp"
#include "robot_msgs/msg/robot_status.hpp"

class DummyRobot_Monitor : public rclcpp::Node
{
  private:
	rclcpp::Subscription<robot_msgs::msg::RobotStatus>::SharedPtr subscription;
	void sub_callback(robot_msgs::msg::RobotStatus::SharedPtr msg)
	{
		RCLCPP_INFO(this->get_logger(), "robot watch: x(%f), y(%f), z(%F), s(%d)",
					msg->position.position.x, msg->position.position.y,
					msg->position.position.z, msg->status);
	}

  public:
	DummyRobot_Monitor(std::string name) : Node(name)
	{
		RCLCPP_INFO(this->get_logger(), "Robot monitor node created with name %s", name.c_str());
		subscription = this->create_subscription<robot_msgs::msg::RobotStatus>(
			"RobotStatus", 10,
			std::bind(&DummyRobot_Monitor::sub_callback, this, std::placeholders::_1));
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<DummyRobot_Monitor>("robot_monitor");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}


